﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace GameLibrary.AI.ShortestPath
{
    [Serializable]
   public class AStar
    {
        #region variables
        double[,] Map;
       Position From;
       Position To;

       HashSet<Position> OpenSet;
       HashSet<Position> ClosedSet;

       Dictionary<Position, AStarNode> BestPath;
        #endregion

       public AStar(double[,] map, Position from, Position to)
       {
           this.Map = map;
           this.From = from;
           this.To = to;
           AStarNode node = new AStarNode(from, new List<Position>(), map[from.X, from.Y]);
           this.OpenSet = new HashSet<Position>();
           this.OpenSet.Add(node.CurrentPosition);
           this.ClosedSet = new HashSet<Position>();
           this.BestPath = new Dictionary<Position, AStarNode>();
           this.BestPath.Add(from, node);
       }

       public List<Position> GetResult()
       {
           while (this.OpenSet.Count > 0)
           {
               AStarNode min = GetNodeWithLowestScore();
               if (min.CurrentPosition == this.To)
                   return min.Path;
               RemoveNodeFromOpenSet(min);
               AddNodeToClosedSet(min);
               AddNeighborNodesToOpenSet(min);
           }
           throw new Exception("Fail to find");
       }

       private AStarNode GetNodeWithLowestScore()
       {
           var nodes = from n in BestPath
                       where OpenSet.Contains(n.Key)
                       select n.Value;
           List<AStarNode> an = new List<AStarNode>(nodes);
           return an.Min();
       }

       private void RemoveNodeFromOpenSet(AStarNode node)
       {
           this.OpenSet.Remove(node.CurrentPosition);
       }

       private void AddNodeToClosedSet(AStarNode node)
       {
           this.ClosedSet.Add(node.CurrentPosition);
       }

       private void AddNeighborNodesToOpenSet(AStarNode node)
       {
           var neighbors = GetNeighborNodes(node);
           foreach (var n in neighbors)
           {
               if (IsInClosedSet(n.CurrentPosition))
                   if (IsSmallerThanExistingNodeValueInBestPath(n) == false)
                       continue;
               if (IsBestPathContains(n.CurrentPosition) == false)
               {
                   AddToBestPath(n);
               }
               else
                   if (IsSmallerThanExistingNodeValueInBestPath(n))
                       ChangeBestPath(n);
               if (IsInOpenSet(n) == false)
                   AddToOpenSet(n);
           }
       }

       private IEnumerable<AStarNode> GetNeighborNodes(AStarNode node)
       {
           for(int i=-1;i<2;i++)
               for(int j =-1;j<2;j++)
           {
               Position pos = node.CurrentPosition + new Position(i, j);
               if (pos.IsLessThanZero()
                   || pos.X >= this.Map.GetLength(0)
                   || pos.Y >= this.Map.GetLength(1)
                   || new Position(i,j) == Position.Zero)
                   continue;

               yield return new AStarNode(pos, node.Path, node.Value + this.Map[pos.X, pos.Y]);
           }
       }

       private bool IsInClosedSet(Position nodePosition)
       {
           return this.ClosedSet.Contains(nodePosition);
       }

       private bool IsSmallerThanExistingNodeValueInBestPath(AStarNode node)
       {
           if (BestPath.ContainsKey(node.CurrentPosition))
               return node.Value < BestPath[node.CurrentPosition].Value;
           else
               return true;
       }

       private bool IsBestPathContains(Position pos)
       {
           return this.BestPath.ContainsKey(pos);
       }

       private void AddToBestPath(AStarNode node)
       {
           this.BestPath.Add(node.CurrentPosition, node);
       }

       private void ChangeBestPath(AStarNode node)
       {
           this.BestPath[node.CurrentPosition] = node;
       }

       private bool IsInOpenSet(AStarNode node)
       {
           return OpenSet.Contains(node.CurrentPosition);
       }
       private void AddToOpenSet(AStarNode node)
       {
           OpenSet.Add(node.CurrentPosition);
       }
   }
}
